#include "WProgram.h"
#include "ServoC.h"

ServoC::ServoC(uint8_t pinArg, uint8_t angleArg, unsigned long mintime, unsigned long maxtime) 
{
    pin = pinArg;
    minpulse = mintime;
    maxpulse = maxtime;
    angle = angleArg;
    pulse0 = minpulse + (maxpulse-minpulse)* (float)(angle/180.0);
    risefalltime = 0;
    pinMode(pin,OUTPUT);
    digitalWrite(pin, LOW);
}

//0<angleArg<180
void ServoC::write(int angleArg)
{
    angle = angleArg;
    pulse0 = minpulse + (maxpulse-minpulse)* (float)(angle/180.0);
}

unsigned long ServoC::readpulse()
{
    return pulse0;
}

int ServoC::readangle()
{
    return angle;
}


void ServoC::refresh()
{
     
     if (lastedgerise)
     {
         if( (micros() - risefalltime) > pulse0)
         {
             digitalWrite(pin, LOW);
             risefalltime=micros();
             lastedgerise=false;
         }
     }
     else
     {
         if( (micros() - risefalltime) > 20000)
         {
             digitalWrite(pin, HIGH);
             risefalltime=micros();
             lastedgerise=true;
         }
     }
     
     /*
     digitalWrite(pin, HIGH);
     delayMicroseconds(pulse0);
     digitalWrite(pin, LOW);
     delayMicroseconds(20000);*/
     
}

